/********************************************************************
* Description: trivkins.c
*   Trivial kinematics for 3 axis Cartesian machine
*
*   Derived from a work by Fred Proctor & Will Shackleford
*
* Author:
* License: GPL Version 2
* System: Linux
*    
* Copyright (c) 2004 All rights reserved.
*
* Last change:
********************************************************************/

#include "kinematics.h" /* these decls */


int kinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags)
{
   pos->tran.x = joints[0];
   pos->tran.y = joints[1];
   pos->tran.z = joints[2];
   pos->a = joints[3];
   pos->b = joints[4];
   pos->c = joints[5];
   pos->u = joints[6];
   pos->v = joints[7];
   pos->w = joints[8];

   return 0;
}

int kinematicsInverse(const EmcPose * pos, double *joints, const KINEMATICS_INVERSE_FLAGS * iflags, KINEMATICS_FORWARD_FLAGS * fflags)
{
   joints[0] = pos->tran.x;
   joints[1] = pos->tran.y;
   joints[2] = pos->tran.z;
   joints[3] = pos->a;
   joints[4] = pos->b;
   joints[5] = pos->c;
   joints[6] = pos->u;
   joints[7] = pos->v;
   joints[8] = pos->w;

   return 0;
}

/* implemented for these kinematics as giving joints preference */
int kinematicsHome(EmcPose * world, double *joint, KINEMATICS_FORWARD_FLAGS * fflags, KINEMATICS_INVERSE_FLAGS * iflags)
{
   *fflags = 0;
   *iflags = 0;

   return kinematicsForward(joint, world, fflags, iflags);
}

KINEMATICS_TYPE kinematicsType()
{
   return KINEMATICS_IDENTITY;
}
